function [score,match_indexes1,match_indexes2,G,V,E,A]=EMGaussianMatch(cluster1,cluster2,sigma,miu,threshold)

score=1;
disp("开始计算V");
[G,V,E]=CalculateV(cluster1,cluster2,sigma);
disp("V计算完毕");
disp("开始计算Z");
v1= gather(V{1});
v2=gather(V{2});
%计算V的中心聚簇点W
wd = CalculateW(v1,cluster1);
wm = CalculateW(v2,cluster2);
% centroidG1=CalculateGv2(wd,sigma);
centroidG2=CalculateGv2(wm,sigma);
%再次进行特征值分解
% disp("对centroidG1进行特征值分解");
% [ud,d1]=CalU(centroidG1);
% disp("对centroidG1特征值分解完毕");
disp("对centroidG2进行特征值分解");
[um,d2]=CalU(centroidG2);
disp("对centroidG2特征值分解完毕");

%EM算法
wdLen=size(wd,2);
wmLen=size(wm,2);
alpha=ones(1,wmLen).*(1/wmLen); %初始权重alpha
disp(alpha)
A = [1 0 0
        0 1 0
        0 0 1];%初始仿射变换矩阵A
S=0;
iter_num=100;%设置迭代次数
for iter=1:iter_num
    
    wdN= A*wd;%wd作仿射变换
    [udN,d1]=CalU(CalculateGv2(wdN,sigma));%根据wdN计算udN
    zita = CalZita(udN,um,miu);%计算zita
    R = [1 1 1
        1 1 1
        0 0 0]; %R的定义
%     epsilons  = wm-wdN;%计算仿射变换后的点之间误差矩阵
    pdm =CalDensity(wd,wm);%计算密度
    Pmd = CalP(wd,wm,pdm,alpha);%计算条件后验概率
    alpha = UpdateAlpha(wd,wm,Pmd);%更新α
    [A,S]=UpdateA(Pmd,zita,wd,wm,R);%更新An
    
%     disp(iter);
%     disp(sum(alpha));
end

disp("查找匹配点");
[match_indexes1,match_indexes2]= Match(S,wdLen,wmLen,threshold);







function zita = CalZita(ud,um,miu)
udLen=size(ud,2);
umLen=size(um,2);
total=0;
subProb=zeros(udLen,umLen);
zita=zeros(udLen,umLen);
%先算出分母
for i=1:udLen
    for j=1:umLen
        subProb(i,j)=exp(-miu*norm(ud(i,:)-um(j,:))^2);
        total = total +subProb(i,j);
    end
end
%再算概率
for i=1:udLen
    for j=1:umLen
        zita(i,j)=subProb(i,j)/total;
    end
end


function W = CalculateW(v1,cluster)
[rows,columns]=size(v1);
%V为方阵，对应n12列即存在n12个中心点
W={};
for i=1:columns
    sum1=0;
    sumW=[0,0,0]';
    for j=1:rows
        len = norm(v1(j,i));
        sumW=sumW+cluster(:,i).*len;
        sum1=sum1+len;
    end
    W{i}=sumW./sum1;
end
W = cell2mat(W);

function p = CalDensity(WdN,Wm)
wdLen=size(WdN,2);
wmLen=size(Wm,2);
p=zeros(wdLen,wmLen);

% t= cov(cat(2,Wm(1:2,j),WdN(1:2,i))');
% sigma = cov((Wm-WdN)')+eye(3)*(1e-6);
% sigma = cov([Wm(:,j)';WdN(:,i)'])+eye(3)*(1e-6);

for i=1:wdLen
    for j=1:wmLen
        sigma = cov([Wm(:,j)';WdN(:,i)'])+eye(3)*(1e-6);
        invCm = inv(sigma);
        epsilon = Wm(:,j)-WdN(:,i);
        p(i,j) = exp(-0.5*epsilon'*invCm*epsilon)/(2*pi*sqrt(norm(sigma)));
    end
end


function Pmd = CalP(wd,wm,pdm,alpha)
wdLen=size(wd,2);
wmLen =size(wm,2);
% Pdm=zeros(wdLen,wmLen);
Pmd=zeros(wmLen,wdLen);
%计算 wm of wd
for i=1:wdLen
    %先计算分母部分的全概率
    total=0;
    for j=1:wmLen
        total = total+(alpha(j)*pdm(i,j));
    end
%     total= sum(sum(pdm.*alpha));
    %再计算pmd
    for j=1:wmLen
        Pmd(j,i) = (alpha(j)*pdm(i,j))/total;
    end
end



function alpha = UpdateAlpha(wd,wm,Pmd)
wdLen=size(wd,2);
wmLen=size(wm,2);
alpha= zeros(1,wmLen);
for j=1:wmLen
       alpha(j)=sum(Pmd(j,:))/wdLen;
end

function [matchIndex1,matchIndex2]= Match(S,WdLen,WmLen,threshold)
    matchIndex1 = {};
    matchIndex2 = {};
    match_count=0;
match_flag=zeros(WdLen,WmLen);
for i=1:WdLen
     [max_val,max_index]=max(S(i,:));
    for j=1:WmLen
         [max_val2,max_index2]=max(S(:,j));
            if(max_val2==S(i,j)&&S(i,j)<threshold&&match_flag(i,j)==0&&ismember(1,match_flag(:,j))==0)
                match_count=match_count+1;
                matchIndex1{match_count}=i;
                matchIndex2{match_count}=j;
                match_flag(i,j)=1;
                match_flag(j,i)=1;
                break;
            end
    end
end          
% disp(["图样1和图样2","之间的匹配特征数为",num2str(match_count),"匹配率为",num2str(score)]);
    

function [A,S]= UpdateA(Pmd,zita,Wd,Wm,R)
left=0;
right=0;
wdLen=size(Wd,2);
wmLen=size(Wm,2);
S=zeros(wdLen,wmLen);
%        sigma = cov([Wm(:,j)';Wd(:,i)'])+eye(3)*(1e-6);
% sigma = cov((Wm-Wd)')+eye(3)*(1e-6);
% invCm = inv(sigma);
for i=1:wdLen
    for j=1:wmLen
sigma = cov([Wm(:,j)';Wd(:,i)'])+eye(3)*(1e-6);
invCm = inv(sigma);
       S(i,j)=Pmd(j,i)*zita(i,j);
       left = left+S(i,j)*Wd(:,i)'*R'*Wd(:,i)*invCm;
       right = right+S(i,j)*Wm(:,j)'*R'*Wd(:,i)*invCm;
    end
end
A=inv(left+eye(3)*(1e-6))*right;




function  G = CalculateGv2(cluster,sigma)
[n1,m1]=size(cluster);
G=zeros(m1,m1);
residual=0.01;
for i=1:m1
    for j=1:m1
%         disp([i,j]);
        dist = norm(cluster(:,i)-cluster(:,j));
%         k=-norm(cluster(:,i)-cluster(:,j))^2/(2*sigma);
%         G(i,j)=exp(k);
%         dist =  sqrt((cluster(i,1)-cluster(j,1))^2+(cluster(i,2)-cluster(j,2))^2);
        G(i,j)=2/(dist*pi+residual)*tanh(pi*dist/sigma);
    end
end

function [u,d]= CalU(centroidG)
centroidG=gpuArray(centroidG);
[u,d]=eig(centroidG);
[e,index] = sort(diag(d),'descend');
d = diag(e);
u = gather(u(:,index));

function  [G,V,E] = CalculateV(cluster1,cluster2,sigma)
G={};
V={};
E={};
disp("计算H1");
g1=CalculateGv2(cluster1,sigma);
disp("计算H2");
g2=CalculateGv2(cluster2,sigma);
% reflectIndex =ResortG(g1,g2);
% g2=CalculateGv2(cluster2(:,reflectIndex),sigma);
disp("H计算完毕");
disp(g1);


disp("矫正H完毕");

disp("对H1进行特征值分解");

g1=gpuArray(g1);
[v1,d1]=eig(g1);
[e1,index] = sort(diag(d1),'descend');
d1 = diag(e1);
v1 = v1(:,index);
% v1 = v1(:,index).*e1';

disp("对H1特征值分解完毕");


% v1=fliplr(v1)
% e1=flip(diag(d1));
% d1=diag(e1);
g2=gpuArray(g2);
disp("对H2进行特征值分解");
[v2,d2]=eig(g2);
[e2,index] = sort(diag(d2),'descend');
d2 = diag(e2);
% v2 = v2(:,index);
% v2 = v2(:,index).*e2';
disp("对H2特征值分解完毕");
% v2=fliplr(v2)
% e2=flip(diag(d2));
% d2=diag(e2);

[n11,n12]=size(v1);
[n21,n22]=size(v2);
len = min(n11,n21);
v1=v1(:,1:len);
v2=v2(:,1:len);
% [v2,newIndexes] = ResortV(v1,v2); 

% e2=e2';
% e1=e1';
% disp(newIndexes);
% e2 = e2(:,newIndexes);
e1 =e1(1:len,:)';
e2=e2(1:len,:)';
disp(size(e2));
disp(size(v2));
% v1=v1.*e1;
% v2=v2.*e2;
% v1=v1.*(1-1./exp(e1));
% v2=v2.*(1-1./exp(e2));


V={v1,v2};
E={e1,e2};
G={g1,g2};
% [a,b]=sort(e1,'descend');
% [r,c]=ind2sub(e1,b);
% 
% [a,b]=sort(e2,'descend');
% [r,c]=ind2sub(e2,b);


